#include "mainwindow.h"
#include "agv.h"
#include "agv_system.h"
#include "spdlog/spdlog.h"
#include "src/AGVThreadController.h"
#include "src/LogListPanel/LogListPanel.h"
#include "src/TestPicShowWidget.h"
#include "src/agvmodule_defines.h"
#include "third_party/agv_imu_requester/AGVFrameHelper.h"
#include "third_party/agv_imu_requester/AGVRequester.h"
#include "third_party/mtcp-read-write/src/MTcpFrame.h"
#include "ui_mainwindow.h"
#include <cstdint>
#include <opencv2/core/mat.hpp>
#include <qcoreapplication.h>
#include <qglobal.h>
#include <qgridlayout.h>
#include <qhostaddress.h>
#include <qmessagebox.h>
#include <qobject.h>
#include <qpushbutton.h>
#include <qtcpsocket.h>

#include "log.h"

// Global Show Pic Function
static QObject *s_p_mainwindow = nullptr;
void Global_ShowPicOnQt(int window_num, const cv::Mat &mat) {
  cv::Mat copy = mat.clone();

  if (s_p_mainwindow) {
    QCoreApplication::postEvent(
        s_p_mainwindow, new ShowPicEvent(ShowPicEventData{window_num, copy}));
  }
}

MainWindow::MainWindow(QWidget *parent)
    : QMainWindow(parent), ui(new Ui::MainWindow) {
  s_p_mainwindow = this;

  auto font = this->font();
  font.setFamily("Consolas");
  this->setFont(font);

  ui->setupUi(this);

  test_pic_show_widget_ = new TestPicShowWidget(nullptr);
  connect(ui->pb_testpic, &QPushButton::clicked, this, [this]() {
    test_pic_show_widget_->hide();
    test_pic_show_widget_->show();
  });

  auto gridlayout = new QGridLayout(ui->frame_loglist);
  log_list_panel_ = new LogListPanel(nullptr);
  gridlayout->addWidget(log_list_panel_);

  //! 0
  agv_requester_ = new AGVRequester(QHostAddress{"192.168.192.5"}, 19204, this);
  connect(agv_requester_, &AGVRequester::sig_agvframe_received_jo, this,
          [this](uint16_t frame_type, const json::object &jo) {
            switch (frame_type) {
            case 11004: {
              // locate return
              //   spdlog::debug("locate return, json: {}", jo.dumps());

              try {
                cached_agv_rtn_locate_data_ = (agv_return_msg_locate_t)(jo);
              } catch (const std::exception &e) {
                spdlog::error("agv_return_msg_locate_t convert error: {}",
                              e.what());
                break;
              }
              break;
            }
            default: {
              spdlog::warn("Unknown frame type: {}", frame_type);
              break;
            }
            }
          });
  connect(agv_requester_, &AGVRequester::sig_socket_state_changed,
          [this](QTcpSocket::SocketState s) {
            static bool previous_connected = false;

            if (s == QTcpSocket::ConnectedState) {
              spdlog::info("AGVRequester Socket Connected");
              previous_connected = true;
            }
            ui->lb_dis_agv_socket_state->setText(
                s == QTcpSocket::ConnectedState ? tr("已连接") : tr("未连接"));

            if (s == QTcpSocket::UnconnectedState && previous_connected) {
              spdlog::warn("AGVRequester Socket DisConnected");
              spdlog::trace("Cleanup AGV Tcp");
              this->agv_thread_ctrler_->CleanUpAgvTcp();

              previous_connected = false;
            }
          });

  //! 1
  agv_thread_ctrler_ = new AGVThreadController(this);
  _init_agv_signal_slots();

  //! 2
  tcp_rw_ctrler_ = new MTcpReadWriteController(
      QHostAddress::LocalHost, AGVMODULE_MODULE_SERVER_PORT, // listening port
      QHostAddress::LocalHost, AGVMODULE_UPPER_SERVER_PORT, // remote upper port
      this);

  connect(tcp_rw_ctrler_,
          &MTcpReadWriteController::sig_client_socket_state_changed, this,
          [this](QTcpSocket::SocketState s) {
            // qDebug() << "Tcp Writer Client Socket State Changed:" << s;
            ui->lb_dis_client_socket_state->setText(
                s == QTcpSocket::ConnectedState ? tr("已连接") : tr("未连接"));
            ui->lb_dis_client_socket_state->setStyleSheet(
                s == QTcpSocket::ConnectedState
                    ? "QLabel{background-color: rgb(173, 255, 47);}"
                    : "QLabel{background-color: rgb(255, 100, 0);}");

            if (s == QTcpSocket::ConnectedState) {
              spdlog::info("Tcp Writer Client Socket Connected");
              {
                // 连接后最小化, 但仅最小化一次
                static bool has_minimized_once = false;
                if (!has_minimized_once) {
                  this->showMinimized();
                  has_minimized_once = true;
                }
              }
            }
          });

  connect(tcp_rw_ctrler_,
          &MTcpReadWriteController::sig_server_client_num_changed, this,
          [this](int client_num) {
            spdlog::info("Tcp Reader Server, connected client num changed: {}",
                         client_num);
            ui->sb_dis_server_client_num->setValue(
                tcp_rw_ctrler_->server_client_num());
          });

  connect(tcp_rw_ctrler_, &MTcpReadWriteController::sig_server_recv_frame, this,
          [this](const MTcpFrame &frame) {
            uint8_t frameid = frame.frame_id();

            if (frameid != 0) {
              spdlog::info("Server Received frame id: {}", frameid);
            }
            _process_frame(frame);
          });

  //! 3
  info_return_timer_ = new QTimer(this);
  connect(info_return_timer_, &QTimer::timeout, this, [this]() {
    agvmodule_rx_regularinfo_data_t info;

    auto agv_info = AGVSystem::getUpperInfo(cached_agv_rtn_locate_data_.x,
                                            cached_agv_rtn_locate_data_.y,
                                            cached_agv_rtn_locate_data_.angle);
    // 访问agv提供的全局变量

    // 这三个自己去获取, 不走视觉模块
    // spdlog::debug("x: {}, y: {}, angle: {}", cached_agv_rtn_locate_data_.x,
    //               cached_agv_rtn_locate_data_.y,
    //               cached_agv_rtn_locate_data_.angle);
    info.agv_axis_mm_times_1000[0] =
        cached_agv_rtn_locate_data_.x * 1000.0 * 1000.0;
    info.agv_axis_mm_times_1000[1] =
        cached_agv_rtn_locate_data_.y * 1000.0 * 1000.0;
    info.agv_angle_times_1000000 =
        cached_agv_rtn_locate_data_.angle * 1000000.0;
    info.locate_confidence_times_1000 =
        cached_agv_rtn_locate_data_.confidence * 1000.0;

    info.agv_global_state = (uint8_t)agv_info.agv_state;
    info.agv_move_step = (uint8_t)agv_info.agv_move_state;
    info.agv_connection_state =
        this->agv_requester_->socket_state() == QTcpSocket::ConnectedState ? 1
                                                                           : 0;

    info.secondlocate_camera_position_times_1000[0] =
        agv_info.head_camera_axis_x_mm * 1000.0;
    info.secondlocate_camera_position_times_1000[1] =
        agv_info.head_camera_axis_y_mm * 1000.0;

    info.movetotarget_thread_status =
        this->agv_thread_ctrler_->is_movetotarget_thread_running() ? 1 : 0;
    info.initialposition_running_status =
        this->agv_thread_ctrler_->is_initialposition_running() ? 1 : 0;
    info.secondlocate_calcing_status =
        this->agv_thread_ctrler_->is_secondlocatecalc_running() ? 1 : 0;
    info.calculate_map_angle_status =
        this->agv_thread_ctrler_->is_calculate_map_angle_running() ? 1 : 0;

    //! TODO 更新ui显示
    ui->dsb_dis_agv_axis_x->setValue(cached_agv_rtn_locate_data_.x * 1000.0);
    ui->dsb_dis_agv_axis_y->setValue(cached_agv_rtn_locate_data_.y * 1000.0);
    ui->dsb_dis_agv_axis_angle->setValue(cached_agv_rtn_locate_data_.angle);
    ui->lb_dis_agv_globalstate->setText(AGVStateToString(agv_info.agv_state));
    ui->lb_dis_agv_movestep->setText(
        AGVMoveStepToString(agv_info.agv_move_state));
    ui->dsb_dis_camera_axis_x->setValue(agv_info.head_camera_axis_x_mm);
    ui->dsb_dis_camera_axis_y->setValue(agv_info.head_camera_axis_y_mm);
    ui->dsb_dis_agv_confidence->setValue(
        cached_agv_rtn_locate_data_.confidence);

    ui->lb_dis_moveToTargetThreadRunning->setText(
        QString::number(info.movetotarget_thread_status));
    ui->lb_dis_InitialPositionRunning->setText(
        QString::number(info.initialposition_running_status));
    ui->lb_dis_SecondLocateRunning->setText(
        QString::number(info.secondlocate_calcing_status));
    ui->lb_dis_CalculateMapAngleRunning->setText(
        QString::number(info.calculate_map_angle_status));

    ui->lb_dis_agv_socket_state->setText(agv_requester_->socket_state() ==
                                                 QTcpSocket::ConnectedState
                                             ? tr("已连接")
                                             : tr("未连接"));
    ui->lb_dis_agv_socket_state->setStyleSheet(
        agv_requester_->socket_state() == QTcpSocket::ConnectedState
            ? "QLabel{background-color: rgb(173, 255, 47);}"
            : "QLabel{background-color: rgb(255, 100, 0);}");

    auto frame = MTcpFrame::from_data<agvmodule_rx_regularinfo_data_t>(
        AGVModuleFrameRxId_RegularInfo, info);
    this->tcp_rw_ctrler_->send_frame(frame);
  });

  info_return_timer_->start(500);

  request_locate_timer_ = new QTimer(this);
  connect(request_locate_timer_, &QTimer::timeout, this,
          [this]() { _request_locate(); });
  request_locate_timer_->start(500);

  _init_test_buttons();
}

MainWindow::~MainWindow() {
  delete ui;

  delete test_pic_show_widget_;
}

int MainWindow::send_frame_to_upper(const MTcpFrame &frame) {
  return this->tcp_rw_ctrler_->send_frame(frame);
}

void MainWindow::_init_agv_signal_slots() {
  connect(agv_thread_ctrler_, &AGVThreadController::sig_InitialPositionResult,
          this, [this](initialposition_result_t result) {
            agvmodule_rx_initialPositionOnceOver_data_t data;
            data.result_times_1000 = result.result * 1000.0;
            data.error_code = (int)result.err_code;

            ui->sb_initialposition_errorcode->setValue((int)result.err_code);
            ui->dsb_initialposition_score->setValue(result.result);

            auto frame = MTcpFrame::from_data(
                AGVModuleFrameRxId_InitialPositionOnceOver, data);

            auto msg = fmt::format("sig_InitialPositionResult send frame back "
                                   "to upper, score: {}, error_code: {}",
                                   result.result, (int)result.err_code);
            spdlog::trace(msg);
            this->tcp_rw_ctrler_->send_frame(frame);
          });

  connect(agv_thread_ctrler_, &AGVThreadController::sig_SecondLocateMovePlc,
          this, [this](secondlocatecalc_result_t result) {
            agvmodule_rx_secondLocateMovePlc_data_t data;
            data.axis1_inc = result.axis1_inc;
            data.axis2_inc = result.axis2_inc;
            data.error_code = (int)result.err_code;

            ui->sb_secondlocate_errorcode->setValue((int)result.err_code);
            ui->sb_secondlocate_axis_1->setValue(result.axis1_inc);
            ui->sb_secondlocate_axis_2->setValue(result.axis2_inc);

            auto frame = MTcpFrame::from_data(
                AGVModuleFrameRxId_SecondLocateMovePlc, data);

            auto msg = fmt::format(
                "sig_SecondLocateMovePlc send frame back to upper, "
                "axis1_inc: {}, axis2_inc: {}, error_code: {}",
                result.axis1_inc, result.axis2_inc, (int)result.err_code);
            spdlog::trace(msg);
            this->tcp_rw_ctrler_->send_frame(frame);
          });

  connect(agv_thread_ctrler_, &AGVThreadController::sig_CalculateMapAngleResult,
          this, [this](CalculateMapAngleResponse result) {
            auto frame = MTcpFrame::from_data(
                AGVModuleFrameRxId_CalculateMapAngleResult, result);

            ui->sb_calculatemapangle_errorcode->setValue((int)result.errCode);
            ui->sb_calculatemapangle_point_0->setValue(result.points[0]);
            ui->sb_calculatemapangle_point_1->setValue(result.points[1]);
            ui->sb_calculatemapangle_point_2->setValue(result.points[2]);
            ui->sb_calculatemapangle_point_3->setValue(result.points[3]);

            auto msg = fmt::format(
                "sig_CalculateMapAngleResult send frame back to upper, "
                "points: [{}, {}, {}, {}], errCode: {}",
                result.points[0], result.points[1], result.points[2],
                result.points[3], (int)result.errCode);
            spdlog::trace(msg);
            this->tcp_rw_ctrler_->send_frame(frame);
          });
}

void MainWindow::_init_test_buttons() {
  connect(ui->pb_closeprogram, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_quitProgram_data_t data;
    data.reserved = 1;
    auto frame = MTcpFrame::from_data(AGVModuleFrameTxId_QuitProgram, data);

    _process_frame(frame);
  });

  connect(ui->pb_startMoveToTargetThread, &QPushButton::clicked, this,
          [this]() {
            agvmodule_tx_startMoveToTargetThread_data_t data;
            data.reserved = 1;
            auto frame = MTcpFrame::from_data(
                AGVModuleFrameTxId_StartMoveToTargetThread, data);

            _process_frame(frame);
          });

  connect(ui->pb_LineMove, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_startmanuallinemove_data_t data;
    data.inc_pos_mm_times_1000 = ui->dsb_manual_pos->value() * 1000;
    data.target_speed_mm_s_times_1000 = ui->dsb_manual_speed->value() * 1000;
    auto frame =
        MTcpFrame::from_data(AGVModuleFrameTxId_StartManualLineMove, data);

    _process_frame(frame);
  });

  connect(ui->pb_RotateMove, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_startmanualrotatemove_data_t data;
    data.inc_radius_times_1000000 = ui->dsb_manual_pos->value() * 1000000;
    data.target_speed_radius_s_times_1000000 =
        ui->dsb_manual_speed->value() * 1000000;
    auto frame =
        MTcpFrame::from_data(AGVModuleFrameTxId_StartManualRotateMove, data);

    _process_frame(frame);
  });

  connect(ui->pb_StopManualMove, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_stopManualMove_data_t data;
    data.reserved = 1;
    auto frame = MTcpFrame::from_data(AGVModuleFrameTxId_StopManualMove, data);

    _process_frame(frame);
  });

  connect(ui->pb_StopAgvSystem, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_stopSystem_data_t data;
    data.reserved = 1;
    auto frame = MTcpFrame::from_data(AGVModuleFrameTxId_StopSystem, data);

    _process_frame(frame);
  });

  connect(ui->pb_InitialPosition, &QPushButton::clicked, this, [this]() {
    // 提示框提示用户是否真的要InitialPosition
    auto ret = QMessageBox::question(
        this, "InitialPosition", "是否真的要InitialPosition?",
        QMessageBox::Yes | QMessageBox::No, QMessageBox::Yes);
    if (ret != QMessageBox::Yes) {
      return;
    }

    agvmodule_tx_initialPosition_data_t data;
    data.timeout_ms = ui->sb_initialPositionTimeout->value();
    data.x_mm_times_1000 = ui->dsb_ip_x->value() * 1000;
    data.y_mm_times_1000 = ui->dsb_ip_y->value() * 1000;
    data.angle_times_1000000 = ui->dsb_ip_angle->value() * 1000000;
    auto frame = MTcpFrame::from_data(AGVModuleFrameTxId_InitialPosition, data);

    _process_frame(frame);
  });

  connect(ui->pb_FinishInitialPosition, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_finishInitialPosition_data_t data;
    data.reserved = 1;
    auto frame =
        MTcpFrame::from_data(AGVModuleFrameTxId_FinishInitialPosition, data);

    _process_frame(frame);
  });

  connect(ui->pb_BackToZero, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_backToZero_data_t data;
    data.x_mm = ui->sb_backtozero_x_mm->value();
    data.y_mm = ui->sb_backtozero_y_mm->value();
    data.threshold = ui->sb_backtozero_threshold->value();
    auto frame = MTcpFrame::from_data(AGVModuleFrameTxId_BackToZero, data);

    _process_frame(frame);
  });

  connect(ui->pb_SetTarget, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_startFirstLocateSetTarget_data_t data;
    data.x_mm_times_1000 = ui->dsb_settarget_x->value() * 1000;
    data.y_mm_times_1000 = ui->dsb_settarget_y->value() * 1000;
    auto frame = MTcpFrame::from_data(
        AGVModuleFrameTxId_StartFirstLocateSetTarget, data);

    _process_frame(frame);
  });

  connect(ui->pb_SecondLocate, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_startSecondLocateCalc_data_t data;
    data.type = ui->sb_secondlocate_type->value();
    data.timeOut = ui->sb_SecondCalcTimeout_2->value();

    data.binaryThreshold1 = ui->sb_binary_threshold->value();
    data.maxRadius1 = ui->sb_max_radius->value();
    data.binaryThreshold2 = ui->sb_binary_threshold_2->value();
    data.maxRadius2 = ui->sb_max_radius_2->value();

    data.real_radius = ui->sb_secondlocate_realradius->value();
    data.scale_times_1000 = ui->sb_secondlocate_scale->value();

    auto frame =
        MTcpFrame::from_data(AGVModuleFrameTxId_StartSecondLocateCalc, data);

    _process_frame(frame);
  });

  connect(ui->pb_StartBackChange, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_startBackChange_data_t data;
    data.x_mm_times_1000 = ui->dsb_startbackchange_x->value() * 1000.0;
    data.y_mm_times_1000 = ui->dsb_startbackchange_y->value() * 1000.0;

    auto frame = MTcpFrame::from_data(AGVModuleFrameTxID_StartBackChange, data);

    _process_frame(frame);
  });

  connect(ui->pb_calculatemapangle, &QPushButton::clicked, this, [this]() {
    agvmodule_tx_calculateMapAngle_data_t data;
    data.timeOut = ui->sb_calculatemapangle_timeout->value();
    data.binaryThreshold = ui->sb_calculatemapangle_binary_threshol->value();
    data.maxRadius = ui->sb_calculatemapangle_max_radius->value();

    auto frame =
        MTcpFrame::from_data(AGVModuleFrameTxID_CalculateMapAngle, data);

    _process_frame(frame);
  });
}

QString MainWindow::TcpStateToString(QTcpSocket::SocketState s) {
  switch (s) {
  case QTcpSocket::UnconnectedState:
    return "UnconnectedState";
  case QTcpSocket::HostLookupState:
    return "HostLookupState";
  case QTcpSocket::ConnectingState:
    return "ConnectingState";
  case QTcpSocket::ConnectedState:
    return "ConnectedState";
  case QTcpSocket::BoundState:
    return "BoundState";
  case QTcpSocket::ClosingState:
    return "ClosingState";
  default:
    return "Unknown";
  }
}

QString MainWindow::AGVStateToString(State s) {
  switch (s) {
#define CASE_STATE(s) \
  case State::s:      \
    return #s

    CASE_STATE(INIT);
    CASE_STATE(INITPOSITION);
    CASE_STATE(INIT_RUNNING);
    CASE_STATE(TEST);
    CASE_STATE(TEST_RUNNING);
    CASE_STATE(CAPTURE_IMAGE);
    CASE_STATE(CAPTURE_IMAGE_RUNNING);
    CASE_STATE(FEATURE_DETECT);
    CASE_STATE(FEATURE_DETECT_RUNNING);
    CASE_STATE(MATCHING);
    CASE_STATE(MATCHING_RUNNING);
    CASE_STATE(MAPPING);
    CASE_STATE(MAPPING_RUNNING);
    CASE_STATE(MOVING);
    CASE_STATE(MOVING_RUNNING);
    CASE_STATE(WAITING_BACK);
    CASE_STATE(WAITING_FOR_TARGET);
    CASE_STATE(FINISHED);

#undef CASE_STATE
  default:
    return "Unknown";
  }
}

QString MainWindow::AGVMoveStepToString(AGVMoveStep s) {
  switch (s) {
#define CASE_STATE(s)  \
  case AGVMoveStep::s: \
    return #s

    CASE_STATE(ROTATE);
    CASE_STATE(STOP);
    CASE_STATE(FORWARD);
    CASE_STATE(BACKFORWARD);
    CASE_STATE(BACKROTATE);

    CASE_STATE(BACKANGLE);
    CASE_STATE(NEW_ROTATE);
    CASE_STATE(NEW_FORWARD);
    CASE_STATE(NEW_FIRST_ROTATE);
    CASE_STATE(NEW_GET_TARGET_ANGLE);
    CASE_STATE(NEW_SECOND_ROTATE);
    CASE_STATE(NEW_SECOND_FORWARD);
    CASE_STATE(BACKCHANGEROTATE);
    CASE_STATE(BACKCHANGEFORWARD);

#undef CASE_STATE
  default:
    return "Unknown";
  }
}

void MainWindow::_request_locate() {
  // spdlog::debug("request 1004");
  agv_requester_->send_request(1004, QByteArray{}); // 1004
}

void MainWindow::_process_frame(const MTcpFrame &frame) {
  uint8_t frameid = frame.frame_id();

  switch (frameid) {
  case AGVModuleFrameTxId_QuitProgram: {
    auto data_ret = frame.get_data<agvmodule_tx_quitProgram_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg = fmt::format("process frame QuitProgram, reserved: {}",
                           data_ret->reserved);

    spdlog::info(msg);

    QCoreApplication::quit();
    break;
  }
  case AGVModuleFrameTxId_StartManualLineMove: {
    auto data_ret = frame.get_data<agvmodule_tx_startmanuallinemove_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg =
        fmt::format("process frame StartManualLineMove, inc: {}, speed: {}",
                    data_ret->inc_pos_mm_times_1000,
                    data_ret->target_speed_mm_s_times_1000);

    spdlog::info(msg);

    agv_thread_ctrler_->AgvStartLineMove(
        (double)data_ret->inc_pos_mm_times_1000 / 1000.0 / 1000.0, // mm to m
        (double)data_ret->target_speed_mm_s_times_1000 / 1000.0 /
            1000.0 // mm/s to m/s
    );             // line
    break;
  }
  case AGVModuleFrameTxId_StartManualRotateMove: {
    auto data_ret = frame.get_data<agvmodule_tx_startmanualrotatemove_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg =
        fmt::format("process frame StartManualRotateMove, inc: {}, speed: {}",
                    data_ret->inc_radius_times_1000000,
                    data_ret->target_speed_radius_s_times_1000000);

    spdlog::info(msg);

    agv_thread_ctrler_->AgvStartRotateMove(
        (double)data_ret->inc_radius_times_1000000 / 1000000.0, // rad
        (double)data_ret->target_speed_radius_s_times_1000000 /
            1000000.0 // rad/s
    );                // rotate
    break;
  }
  case AGVModuleFrameTxId_StopManualMove: {
    auto data_ret = frame.get_data<agvmodule_tx_stopManualMove_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg = fmt::format("process frame StopManualMove, reserved: {}",
                           data_ret->reserved);

    spdlog::info(msg);

    agv_thread_ctrler_->AgvStopMove();
    break;
  }
  case AGVModuleFrameTxId_StopSystem: {
    auto data_ret = frame.get_data<agvmodule_tx_stopSystem_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg = fmt::format("process frame StopSystem, reserved: {}",
                           data_ret->reserved);

    spdlog::info(msg);

    agv_thread_ctrler_->StopAgvSystem();
    break;
  }
  case AGVModuleFrameTxId_InitialPosition: {
    auto data_ret = frame.get_data<agvmodule_tx_initialPosition_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg = fmt::format(
        "process frame InitialPosition, timeout: {}, x: {}, y: {}, angle: {}",
        data_ret->timeout_ms, data_ret->x_mm_times_1000,
        data_ret->y_mm_times_1000, data_ret->angle_times_1000000);

    spdlog::info(msg);

    agv_thread_ctrler_->AgvInitStart(
        (double)(data_ret->x_mm_times_1000) / 1000.0 / 1000.0, // mm to m
        (double)(data_ret->y_mm_times_1000) / 1000.0 / 1000.0, // mm to m
        (double)(data_ret->angle_times_1000000) / 1000000.0,
        data_ret->timeout_ms); // 10s timeout
    break;
  }
  case AGVModuleFrameTxId_FinishInitialPosition: {
    auto data_ret = frame.get_data<agvmodule_tx_finishInitialPosition_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg = fmt::format("process frame FinishInitialPosition, reserved: {}",
                           data_ret->reserved);

    spdlog::info(msg);

    agv_thread_ctrler_->AgvInitStop();
    break;
  }
  case AGVModuleFrameTxId_StartMoveToTargetThread: {
    auto data_ret =
        frame.get_data<agvmodule_tx_startMoveToTargetThread_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg =
        fmt::format("process frame StartMoveToTargetThread, reserved: {}",
                    data_ret->reserved);

    spdlog::info(msg);

    agv_thread_ctrler_->AgvStartMoveToTargetThread();
    break;
  }
  case AGVModuleFrameTxId_BackToZero: {
    auto data_ret = frame.get_data<agvmodule_tx_backToZero_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg = fmt::format("process frame BackToZero, x_mm: {}, y_mm: {}, threshold: {}",
      data_ret->x_mm, data_ret->y_mm, data_ret->threshold);

    spdlog::info(msg);

    agv_thread_ctrler_->AgvBackToZero(*data_ret);
    break;
  }
  case AGVModuleFrameTxId_StartFirstLocateSetTarget: {
    auto data_ret =
        frame.get_data<agvmodule_tx_startFirstLocateSetTarget_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg =
        fmt::format("process frame StartFirstLocateSetTarget, x: {}, y: {}",
                    data_ret->x_mm_times_1000, data_ret->y_mm_times_1000);

    spdlog::info(msg);

    agv_thread_ctrler_->AgvMoveToTarget(
        (double)(data_ret->x_mm_times_1000) / 1000.0, // mm
        (double)(data_ret->y_mm_times_1000) / 1000.0);
    break;
  }
  case AGVModuleFrameTxId_StartSecondLocateCalc: {
    auto data_ret = frame.get_data<agvmodule_tx_startSecondLocateCalc_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg = fmt::format("process frame StartSecondLocateCalc, {}, {}, {}, "
                           "{}, {}, {}, {}, {}",
                           data_ret->type, data_ret->timeOut,
                           data_ret->binaryThreshold1, data_ret->maxRadius1,
                           data_ret->binaryThreshold2, data_ret->maxRadius2,
                           data_ret->real_radius, data_ret->scale_times_1000);

    spdlog::info(msg);

    agv_thread_ctrler_->SecondLocateCalc(*data_ret); // 10s timeout
    break;
  }
  case AGVModuleFrameTxId_Unknow: {
    // 心跳, 不处理
    break;
  }
  case AGVModuleFrameTxID_StartBackChange: {
    auto data_ret = frame.get_data<agvmodule_tx_startBackChange_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg = fmt::format(
        "process frame StartBackChange, x_mm_times_1000: {}, y_mm_times_1000",
        data_ret->x_mm_times_1000, data_ret->y_mm_times_1000);

    spdlog::info(msg);

    agv_thread_ctrler_->AgvStartBackChange(
        (double)(data_ret->x_mm_times_1000) / 1000.0,
        (double)(data_ret->y_mm_times_1000) / 1000.0);
    break;
  }
  case AGVModuleFrameTxID_CalculateMapAngle: {
    auto data_ret = frame.get_data<agvmodule_tx_calculateMapAngle_data_t>();
    if (!data_ret) {
      spdlog::warn("frame data not valid");
      break;
    }

    auto msg = fmt::format(
        "process frame CalculateMapAngle, timeOut: {}, binaryThreshold: {}, "
        "maxRadius: {}",
        data_ret->timeOut, data_ret->binaryThreshold, data_ret->maxRadius);
    
    spdlog::info(msg);

    agv_thread_ctrler_->CalculateMapAngle(*data_ret);
    break;
  }

    //! TODO
  default: {
    qWarning() << "unknown frame id" << frameid;
    break;
  }
  }
}

void MainWindow::closeEvent(QCloseEvent *event) {
  this->test_pic_show_widget_->hide();
}

void MainWindow::customEvent(QEvent *e) {
  auto event_type = e->type();

  switch (event_type) {
  case ShowPicEvent::type: {
    // qDebug() << "ShowPicEvent event";
    e->accept();

    auto casted_e = dynamic_cast<ShowPicEvent *>(e);
    if (!casted_e) {
      qWarning() << "dynamic cast ShowPicEvent failed";
      break;
    }

    // Process casted_e
    this->test_pic_show_widget_->show_pic(casted_e->data().window_num,
                                          casted_e->data().mat);

    break;
  }
  default: {
    e->ignore();
    break;
  }
  }
}
